/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
	\file XMLtoData.cxx
	\brief class implemenation XML parser

						        \author Jack Elston
						 $Date: 2005/02/12 00:20:45 $
*/

#include <unistd.h>
#include <signal.h>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>
#include <math.h>

#include <xercesc/framework/MemBufInputSource.hpp>
#include <xercesc/framework/Wrapper4InputSource.hpp>
#include <xercesc/parsers/XercesDOMParser.hpp>
#include <xercesc/dom/DOM.hpp>
#include <xercesc/sax/HandlerBase.hpp>
#include <xercesc/util/XMLString.hpp>
#include <xercesc/util/PlatformUtils.hpp>

#include "XMLtoData.h"

#include "task.hxx"
#include "stream_subscription.hxx"
#include "capabilities.hxx"
#include "telemetry_limited.hxx"
#include "atmospheric_data.hxx"
#include "flight_plan.hxx"
#include "link_status.hxx"
#include "task_status.hxx"
#include "wind_data.hxx"

#ifdef HAVE_PICCOLO_SDK
  #include "piccoloParser.h"
#endif

XERCES_CPP_NAMESPACE_USE

using std::auto_ptr;
using namespace CUIntegration;
using xml_schema::flags;

	/*<-------Defines--------->*/
	/*<------End Defines------>*/

	/*<---Global Variables---->*/
		/*! \brief determines level of output to user. */
		extern bool verbose;
	/*<-End Global Variables-->*/

XMLtoData_Parser::XMLtoData_Parser(String name) : ThreadedPipe(name)
{
	try {
		xercesc::XMLPlatformUtils::Initialize();
	}
	catch (const XMLException& toCatch) {
		perror("XMLtoData_Parser::init() - could not initialize xerces");
	}
}

ThreadedPipe::commMode XMLtoData_Parser::mode() {return ThreadedPipe::RX;};

/************************************************************\
|                 XMLtoData_Parser::update                   |
\************************************************************/
/*!
	\brief function called periodically by the thread.
	\pre instance of XMLtoData_Parser has been created and initialized
	\post any data in the inetRxFifo will be parsed and output in
	 command format to the SAVTxFifo
 */
void XMLtoData_Parser::update() 
{
	int val, maxFD;
	fd_set readFDs, writeFDs;				// read fd set
	struct timeval timeout;

	FD_ZERO(&readFDs);				// zero read fd set
	FD_ZERO(&writeFDs);				// zero read fd set
	FD_SET(fifo_FDs[OUTBOARD_RX],&readFDs);			// add fifo
	FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);			// add fifo

	fifo_FDs[OUTBOARD_RX] > fifo_FDs[INBOARD_TX] ? maxFD = fifo_FDs[OUTBOARD_RX] : maxFD = fifo_FDs[INBOARD_TX];

	timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

	if( (val = select(maxFD + 1, &readFDs, &writeFDs,NULL,&timeout)) <= 0) {
		if( val != 0 ) {
			if (errno != EINTR) {
				perror("XMLtoData_Parser::update() select");
				execRate = -1;
			} else
				cout << "XMLtoData_Parser::update() - select interrupted" << endl;
		}
	}
	else {
		if(FD_ISSET(fifo_FDs[OUTBOARD_RX], &readFDs))
		{
			int n = 0;
			n=readFifo(OUTBOARD_RX);
			if(n>0) {
				Client * me = shmPtr->clients.getClient(0);
				fifo_msg.src.s_addr = shmPtr->clients.getAddr(me).s_addr;  //FIXME - cheap hack to use as a gateway
				parse(n);
			}
		}
	}
}

void XMLtoData_Parser::parse(int num) 
{
	uint8_t * dataPtr = CMD_DATAPTR(fifo_msg.data);
	int n;
	//bool isvalid=false;;

	XercesDOMParser* parser = new XercesDOMParser();
	parser->setValidationScheme(XercesDOMParser::Val_Never);
	parser->setDoNamespaces(true);

	ErrorHandler* errHandler = (ErrorHandler*) new HandlerBase();
	parser->setErrorHandler(errHandler);

	MemBufInputSource is ((uint8_t*)fifo_msg.data, num, "tempfile", false);
	Wrapper4InputSource wis (&is, false);

	try {
		parser->parse(is);
	}
	catch (const XMLException& toCatch) {
		char* message = XMLString::transcode(toCatch.getMessage());
		cout << "Exception message is: \n"
			<< message << "\n";
		XMLString::release(&message);
		return;
	}
	catch (const DOMException& toCatch) {
		char* message = XMLString::transcode(toCatch.msg);
		cout << "Exception message is: \n"
			<< message << "\n";
		XMLString::release(&message);
		return;
	}
	catch (...) {
		cout << "Unexpected XML Exception \n" ;
		return;
	}


	DOMDocument* document = parser->getDocument();
	//DOMElement* element = document->getDocumentElement();

	try {

		std::auto_ptr<Task::Task_type> data (Task::Task(*document));

		if(!strcmp(data->Type().c_str(),"Track")) {

			VehicleInfo_t * vehicle;
			int payload_size=0;
			int num = 0;

			for (Task::Task_type::Node_const_iterator i (data->Node ().begin ());
					i != data->Node ().end ();
					++i) {

				vehicle = &((VehicleInfo_t *)(dataPtr))[num];
				num++;

				strcpy(vehicle->id,i->ID().get().c_str());
				payload_size += sizeof(VehicleInfo_t);
			}


			String dest = data->Destination().c_str();
			Client * a_client = shmPtr->clients.getClient(dest);

			if(a_client) {
				fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr; 
				CMD_CMD(fifo_msg.data) = CMD_TRACK;
				CMD_NUM(fifo_msg.data) = payload_size;
				CMD_CSUM(fifo_msg.data) = 0;
				CMD_CSUM(fifo_msg.data) = C_calcCheckSum(fifo_msg.data);
				fifo_msg.size = CMD_SIZE(fifo_msg.data);

				n=writeFifo(INBOARD_TX);
			}
			else
				cout << "XMLtoData_Parser::update() - no such client: " << dest << endl;


		}
		else if(!strcmp(data->Type().c_str(),"Chain")) {

			VehicleInfo_t * vehicle;
			int payload_size=0;
			int num = 0;

			for (Task::Task_type::Node_const_iterator i (data->Node ().begin ());
					i != data->Node ().end ();
					++i) {

				vehicle = &((VehicleInfo_t *)(dataPtr))[num];
				num++;

				strcpy(vehicle->id,i->ID().get().c_str());
				payload_size += sizeof(VehicleInfo_t);

			}

			String dest = data->Destination().c_str();
			Client * a_client = shmPtr->clients.getClient(dest);

			if(a_client) {
				fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr; 
				CMD_CMD(fifo_msg.data) = CMD_CHAIN;
				CMD_NUM(fifo_msg.data) = payload_size;
				CMD_CSUM(fifo_msg.data) = 0;
				CMD_CSUM(fifo_msg.data) = C_calcCheckSum(fifo_msg.data);
				fifo_msg.size = CMD_SIZE(fifo_msg.data);

				n=writeFifo(INBOARD_TX);
			}
			else
				cout << "XMLtoData_Parser::update() - no such client: " << dest << endl;
		}
		else if(!strcmp(data->Type().c_str(),"Formation")) {

			VehicleInfo_t * vehicle;
			int payload_size=0;
			int num = 0;

			for (Task::Task_type::Node_const_iterator i (data->Node ().begin ());
					i != data->Node ().end ();
					++i) {

				vehicle = &((VehicleInfo_t *)(dataPtr))[num];
				num++;

				strcpy(vehicle->id,i->ID().get().c_str());
				payload_size += sizeof(VehicleInfo_t);

			}

			String dest = data->Destination().c_str();
			Client * a_client = shmPtr->clients.getClient(dest);

			if(a_client) {
				fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr; 
				CMD_CMD(fifo_msg.data) = CMD_FORMATION;
				CMD_NUM(fifo_msg.data) = payload_size;
				CMD_CSUM(fifo_msg.data) = 0;
				CMD_CSUM(fifo_msg.data) = C_calcCheckSum(fifo_msg.data);
				fifo_msg.size = CMD_SIZE(fifo_msg.data);

				n=writeFifo(INBOARD_TX);
			}
			else
				cout << "XMLtoData_Parser::update() - no such client: " << dest << endl;
		}
		else if(!strcmp(data->Type().c_str(),"OrbitValues")) {
			OrbitValues_t * orbit_values = (OrbitValues_t *)dataPtr;

      orbit_values->rad = data->Values()->Radius().get();
      orbit_values->alt = data->Values()->Altitude().get();
      orbit_values->wp = data->Values()->Waypoint().get();
      orbit_values->dir = data->Values()->Direction().get();
      orbit_values->offset[0] = data->Values()->OffsetNorth().get();
      orbit_values->offset[1] = data->Values()->OffsetEast().get();

			String dest = data->Destination().c_str();
			Client * a_client = shmPtr->clients.getClient(dest);

			if(a_client) {
				fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr; 
				CMD_CMD(fifo_msg.data) = CMD_ORBIT_VALUES;
				CMD_NUM(fifo_msg.data) = sizeof(OrbitValues_t);
				CMD_CSUM(fifo_msg.data) = 0;
				CMD_CSUM(fifo_msg.data) = C_calcCheckSum(fifo_msg.data);
				fifo_msg.size = CMD_SIZE(fifo_msg.data);

				n=writeFifo(INBOARD_TX);
			}
			else
				cout << "XMLtoData_Parser::update() - no such client: " << dest << endl;
		}
		else {

			Waypt_t * waypoint;
			int payload_size=0;
			int num = 0;

			for (Task::Task_type::Waypoint_const_iterator i (data->Waypoint ().begin ());
					i != data->Waypoint ().end ();
					++i) 
			{

				waypoint = &((Waypt_t *)(dataPtr))[num];
				num++;

				waypoint->orbitRad=0;
				waypoint->next=i->SequenceNumber().get()+1;

				if(i+1 == data->Waypoint().end()) {
					if(data->Type()=="Loop") {
						waypoint->next=(data->Waypoint().begin())->SequenceNumber().get();
					} else if(data->Type()=="Loiter") {
						//waypoint->next=30; // FIXME
						waypoint->next=(data->Waypoint().begin())->SequenceNumber().get(); 
						waypoint->orbitRad=100;
					} else {
						waypoint->next=i->SequenceNumber().get()-1; // FIXME
						//waypoint->next=(data->Waypoint().begin())->SequenceNumber().get(); 
						waypoint->orbitRad=100;
					}
				}

				waypoint->number  =i->SequenceNumber().get();
				waypoint->pos.lat =i->Latitude().get();
				waypoint->pos.lon =i->Longitude().get();
				waypoint->pos.alt=1770;

				payload_size += sizeof(Waypt_t);
			}

			if(data->Type()=="Loiter" && num == 1)
			{
				//take care of sent waypoint
				Waypt_t * first_waypoint = &((Waypt_t *)(dataPtr))[0];
				first_waypoint->next=(data->Waypoint().begin())->SequenceNumber().get()+1;

				waypoint = &((Waypt_t *)(dataPtr))[num];
				num++;

				waypoint->orbitRad=0;
				waypoint->next    =first_waypoint->number;
				waypoint->number  =first_waypoint->next;
				waypoint->pos.lat =first_waypoint->pos.lat+0.001;
				waypoint->pos.lon =first_waypoint->pos.lon+0.001;
				waypoint->pos.alt=1770;

				payload_size += sizeof(Waypt_t);
			}

			String dest = data->Destination().c_str();
			Client * a_client = shmPtr->clients.getClient(dest);

			if(a_client) {
				fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr; 
				CMD_CMD(fifo_msg.data) = CMD_FP;
				CMD_NUM(fifo_msg.data) = payload_size;
				CMD_CSUM(fifo_msg.data) = 0;
				CMD_CSUM(fifo_msg.data) = C_calcCheckSum(fifo_msg.data);
				fifo_msg.size = CMD_SIZE(fifo_msg.data);

				n=writeFifo(INBOARD_TX);
			}
			else
				cout << "XMLtoData_Parser::update() - no such client: " << dest << endl;
		}
	}
	catch(::xsd::cxx::tree::unexpected_element<char>  &e)
	{
		// not a Task
	}
	catch(...)
	{
		cout << "Malformatted XML \n" << endl << flush;
		return;
	}

	try {
		std::auto_ptr<StreamSubscribe::StreamSubscribe_type> data (StreamSubscribe::StreamSubscribe(*document));

    String dest = data->VehicleID().c_str();
		Client * a_client = shmPtr->clients.getClient(dest);
    if(!a_client) return;

		StreamReq_t * request = (StreamReq_t*)CMD_DATAPTR(fifo_msg.data);

		if(!strcmp(data->System().ID().c_str(),"Servo"))
			request->id = CMD_SERVO;
		else if(!strcmp(data->System().ID().c_str(),"IMU"))
			request->id = CMD_IMU;
		else if(!strcmp(data->System().ID().c_str(),"Air Data"))
			request->id = CMD_AIR_DATA;
		else if(!strcmp(data->System().ID().c_str(),"GPS"))
			request->id = CMD_GPS;
		else if(!strcmp(data->System().ID().c_str(),"Communications"))
			request->id = CMD_COMM;
		else if(!strcmp(data->System().ID().c_str(),"Payload"))
			request->id = CMD_PAYLOAD;
		else if(!strcmp(data->System().ID().c_str(),"Health and Status"))
			request->id = CMD_H_AND_S;
		else if(!strcmp(data->System().ID().c_str(),"Autopilot"))
			request->id = CMD_AUTOPILOT;
		else if(!strcmp(data->System().ID().c_str(),"FlightComputer"))
			request->id = CMD_FLIGHT_COMPUTER;
		else {cout << "Invalid system ID" << endl; return;}

		switch(request->id) {
		//<!-- SERVO -->
		//<!-- IMU -->
		case CMD_IMU:
			if(!strcmp(data->System().Stream().c_str(),"Angles"))
				request->id |= (CMD_IMU_ANG & 0xF);
			else if(!strcmp(data->System().Stream().c_str(),"Rates"))
				request->id |= (CMD_IMU_RATE & 0xF);
			else if(!strcmp(data->System().Stream().c_str(),"Accelerations"))
				request->id |= (CMD_IMU_ACC & 0xF);
			break;
		//<!-- Air Data -->
		case CMD_AIR_DATA:
			if(!strcmp(data->System().Stream().c_str(),"Wind"))
				request->id |= (CMD_WIND & 0xF);
			break;
		//<!-- GPS -->
		case CMD_GPS:
			if(!strcmp(data->System().Stream().c_str(),"Latitude and Longitude"))
				request->id |= (CMD_GPS_LATLON & 0xF);
			else if(!strcmp(data->System().Stream().c_str(),"Altitude"))
				request->id |= (CMD_GPS_ALT & 0xF);
			else if(!strcmp(data->System().Stream().c_str(),"UTC"))
				request->id |= (CMD_GPS_UTC & 0xF);
			else if(!strcmp(data->System().Stream().c_str(),"Track"))
				request->id |= (CMD_GPS_TRACK & 0xF);
			else if(!strcmp(data->System().Stream().c_str(),"SOG"))
				request->id |= (CMD_GPS_SOG & 0xF);
			else if(!strcmp(data->System().Stream().c_str(),"Accuracy"))
				request->id |= (CMD_GPS_ACCURACY & 0xF);
			else if(!strcmp(data->System().Stream().c_str(),"Telemetry"))
				request->id |= (CMD_GPS_TELEM & 0xF);
			break;
		//<!-- Communications -->
		case CMD_COMM:
			if(!strcmp(data->System().Stream().c_str(),"Link Status"))
				request->id |= (CMD_LINK_STATUS & 0xF);
			break;
		//<!-- Payload -->
		case CMD_PAYLOAD:
			if(!strcmp(data->System().Stream().c_str(),"PTH"))
				request->id |= (CMD_PTH & 0xF);
			else if(!strcmp(data->System().Stream().c_str(),"Sonde"))
				request->id |= (CMD_SONDE & 0xF);
			break;
		//<!-- Health and Status -->
		//<!-- Autopilot -->
		case CMD_AUTOPILOT:
			if(!strcmp(data->System().Stream().c_str(),"Flight Plan"))
				request->id |= (CMD_FP_SUBS & 0xF);
			else if(!strcmp(data->System().Stream().c_str(),"Telemetry"))
				request->id |= (CMD_TELEM & 0xF);
			break;
		//<!-- Flight Computer -->
		}

		request->client.s_addr = shmPtr->clients.getAddr(a_client).s_addr; 
		request->port = 0;

		if(data->System().Action()=="Subscribe") {
			request->send_data = true;
		} else
			request->send_data = false;

    fifo_msg.dest.s_addr = request->client.s_addr;
    CMD_CMD(fifo_msg.data) = CMD_STREAM_REQ;
    CMD_NUM(fifo_msg.data) = sizeof(StreamReq_t);
    CMD_CSUM(fifo_msg.data) = 0;
    CMD_CSUM(fifo_msg.data) = C_calcCheckSum(fifo_msg.data);
    fifo_msg.size = CMD_SIZE(fifo_msg.data);

    n=writeFifo(INBOARD_TX);
	}
	catch(::xsd::cxx::tree::unexpected_element<char>  &e)
	{
		// not a Task
	}
	catch(...)
	{
		cout << "Malformatted XML \n" << endl << flush;
		return;
	}

	try {
		std::auto_ptr<Capabilities::Capabilities_type> data (Capabilities::Capabilities(*document));
    SystemCharacter_t * system;
    int payload_size=0;
    int num = 0;

    VehicleInfo_t * vehicle = (VehicleInfo_t *)dataPtr;
    strcpy(vehicle->id,data->VehicleID().c_str());
    payload_size += sizeof(VehicleInfo_t);

    for (Capabilities::Capabilities_type::System_const_iterator i (data->System ().begin ());
        i != data->System ().end ();
        ++i) {

      system = &((SystemCharacter_t *)(dataPtr+sizeof(VehicleInfo_t)))[num];
      if(!strcmp(i->ID().c_str(),"Servo"))
        system->id = CMD_SERVO;
      else if(!strcmp(i->ID().c_str(), "IMU"))
        system->id = CMD_IMU;
      else if(!strcmp(i->ID().c_str(), "Air Data"))
        system->id = CMD_AIR_DATA;
      else if(!strcmp(i->ID().c_str(), "GPS"))
        system->id = CMD_GPS;
      else if(!strcmp(i->ID().c_str(), "Communications"))
        system->id = CMD_COMM;
      else if(!strcmp(i->ID().c_str(), "Payload"))
        system->id = CMD_PAYLOAD;
      else if(!strcmp(i->ID().c_str(), "Health and Status"))
        system->id = CMD_H_AND_S;
      else if(!strcmp(i->ID().c_str(), "Autopilot"))
        system->id = CMD_AUTOPILOT;
      else if(!strcmp(i->ID().c_str(), "FlightComputer"))
        system->id = CMD_FLIGHT_COMPUTER;

      system->commands = 0;
      system->streams = 0;
      
      if(i->AcceptedCommands()) {
        for (Capabilities::AcceptedCommands::Command_const_iterator j (i->AcceptedCommands().get().Command().begin ());
            j != i->AcceptedCommands().get().Command().end ();
            ++j) {
          if(!strcmp(j->c_str(),"Go to Waypoint"))
            system->commands |= (1 << ((CMD_GOTO_WP & 0x0F) - 1));
          else if(!strcmp(j->c_str(), "Flight Plan"))
            system->commands |= (1 << ((CMD_FP & 0x0F) - 1));
          else if(!strcmp(j->c_str(), "Autopilot Rates"))
            system->commands |= (1 << ((CMD_AP_RATES & 0x0F) - 1));
          else if(!strcmp(j->c_str(), "Coordinate"))
            system->commands |= (1 << ((CMD_COORDINATE & 0x0F) - 1));
          else if(!strcmp(j->c_str(), "Custody"))
            system->commands |= (1 << ((CMD_CUSTODY & 0x0F) - 1));
          else if(!strcmp(j->c_str(), "Track"))
            system->commands |= (1 << ((CMD_TRACK & 0x0F) - 1));
          else if(!strcmp(j->c_str(), "Phase"))
            system->commands |= (1 << ((CMD_PHASE & 0x0F) - 1));
          else if(!strcmp(j->c_str(), "Search"))
            system->commands |= (1 << ((CMD_SEARCH & 0x0F) - 1));
          else if(!strcmp(j->c_str(), "Return"))
            system->commands |= (1 << ((CMD_RETURN & 0x0F) - 1));
          else if(!strcmp(j->c_str(), "Formation"))
            system->commands |= (1 << ((CMD_FORMATION & 0x0F) - 1));
          else if(!strcmp(j->c_str(), "Chain"))
            system->commands |= (1 << ((CMD_CHAIN & 0x0F) - 1));
          else if(!strcmp(j->c_str(), "Chain Gains"))
            system->commands |= (1 << ((CMD_CHAIN_GAINS & 0x0F) - 1));
          else if(!strcmp(j->c_str(), "Orbit Values"))
            system->commands |= (1 << ((CMD_ORBIT_VALUES & 0x0F) - 1));
        }
      }
      
      if(i->AvailableStreams()) {
        for (Capabilities::AvailableStreams::Stream_const_iterator j (i->AvailableStreams().get().Stream().begin ());
            j != i->AvailableStreams().get().Stream().end ();
            ++j) {
          if(!strcmp(j->c_str(),"Angles"))
            system->streams |= (1 << ((CMD_IMU_ANG & 0x0F) - 1));
          else if(!strcmp(j->c_str(), "Rates"))
            system->streams |= (1 << ((CMD_IMU_RATE & 0x0F) - 1));
          else if(!strcmp(j->c_str(), "Accelerations"))
            system->streams |= (1 << ((CMD_IMU_ACC & 0x0F) - 1));
          else if(!strcmp(j->c_str(), "Wind"))
            system->streams |= (1 << ((CMD_WIND & 0x0F) - 1));
          else if(!strcmp(j->c_str(), "Latitude and Longitude"))
            system->streams |= (1 << ((CMD_GPS_LATLON & 0x0F) - 1));
          else if(!strcmp(j->c_str(), "Altitude"))
            system->streams |= (1 << ((CMD_GPS_ALT & 0x0F) - 1));
          else if(!strcmp(j->c_str(), "UTC"))
            system->streams |= (1 << ((CMD_GPS_UTC & 0x0F) - 1));
          else if(!strcmp(j->c_str(), "Track"))
            system->streams |= (1 << ((CMD_GPS_TRACK & 0x0F) - 1));
          else if(!strcmp(j->c_str(), "SOG"))
            system->streams |= (1 << ((CMD_GPS_SOG & 0x0F) - 1));
          else if(!strcmp(j->c_str(), "Accuracy"))
            system->streams |= (1 << ((CMD_GPS_ACCURACY & 0x0F) - 1));
          else if(!strcmp(j->c_str(), "Telemetry") && system->id == CMD_GPS)
            system->streams |= (1 << ((CMD_GPS_TELEM & 0x0F) - 1));
          else if(!strcmp(j->c_str(), "Link Status"))
            system->streams |= (1 << ((CMD_LINK_STATUS & 0x0F) - 1));
          else if(!strcmp(j->c_str(), "PTH"))
            system->streams |= (1 << ((CMD_PTH & 0x0F) - 1));
          else if(!strcmp(j->c_str(), "Sonde"))
            system->streams |= (1 << ((CMD_SONDE & 0x0F) - 1));
          else if(!strcmp(j->c_str(), "Flight Plan"))
            system->streams |= (1 << ((CMD_FP_SUBS & 0x0F) - 1));
          else if(!strcmp(j->c_str(), "Telemetry"))
            system->streams |= (1 << ((CMD_TELEM & 0x0F) - 1));
        }
      }

      /*system->range = i->SensorInfo().get().Range().get();
      system->fov = i->SensorInfo().get().FOV().get();
      system->direction = i->SensorInfo().get().Direction().get();*/

      num++;
      payload_size += sizeof(SystemCharacter_t);

    }

		String dest = data->VehicleID().c_str();
		Client * me = shmPtr->clients.getClient(0);
		Client * a_client = shmPtr->clients.getClient(dest);

		if(!a_client) {
      int i;
      for( i=0; i < MAX_CLIENTS; i++) {
        Client *b_client = shmPtr->clients.getClient(i);
        if( !b_client) break;
      }
      if( i >= MAX_CLIENTS )  {
        cout << "XMLtoData_Parser::update() - no such client: " << dest << endl;
        return;
      }
      fifo_msg.src.s_addr = inet_addr("127.0.0.1") + ((uint8_t)i << 24); ; //we don't know an address
    }
    else
    fifo_msg.src.s_addr = shmPtr->clients.getAddr(a_client).s_addr; 
    fifo_msg.dest.s_addr = shmPtr->clients.getAddr(me).s_addr; 
    CMD_CMD(fifo_msg.data) = CMD_CAPABILITY;
    CMD_NUM(fifo_msg.data) = payload_size;
    CMD_CSUM(fifo_msg.data) = 0;
    CMD_CSUM(fifo_msg.data) = C_calcCheckSum(fifo_msg.data);
    fifo_msg.size = CMD_SIZE(fifo_msg.data);

    n=writeFifo(INBOARD_TX);
  }
	catch(::xsd::cxx::tree::unexpected_element<char>  &e){}// not matched
	catch(...){cout << "Malformatted XML \n" << endl << flush; return;}

	try {
		std::auto_ptr<Telemetry::Telemetry_type> data (Telemetry::Telemetry(*document));

		String dest = data->VehicleID().c_str();
		Client * me = shmPtr->clients.getClient(0);
		Client * a_client = shmPtr->clients.getClient(dest);
    if(a_client == NULL){
      cout << "XMLtoData_Parser::update() - no such client: " << dest << endl;
      return;
    }

		if(a_client->capabilities.find(CMD_TELEM)) {
      Telemetry_t * telemetry = (Telemetry_t *)dataPtr;

      telemetry->gps.pos.lat = data->GPS().Latitude().get();
      telemetry->gps.pos.lon = data->GPS().Longitude().get();
      telemetry->gps.track = data->GPS().Track().get();
      telemetry->gps.pos.alt = data->GPS().Height().get();
      telemetry->gps.sog = data->GPS().SOG().get();
      telemetry->gps.utc.hours = data->GPS().UTCHours().get();
      telemetry->gps.utc.minutes = data->GPS().UTCMinutes().get();
      telemetry->gps.utc.seconds = data->GPS().UTCSeconds().get();

      telemetry->airdata.alt = data->AirData().BarometricAlt().get();
      telemetry->airdata.tas = data->AirData().TrueAirSpeed().get();

      telemetry->angles.roll = (int16_t)((data->InertialMeasurements().RollAngle().get())*M_PI/0.18);
      telemetry->angles.pitch = (int16_t)((data->InertialMeasurements().PitchAngle().get())*M_PI/0.18);
      telemetry->angles.yaw = (int16_t)((data->InertialMeasurements().YawAngle().get())*M_PI/0.18);

      telemetry->navigation.waypt = data->ControlLoops().CommandedWaypoint().get();

      telemetry->system.twelveV = (uint16_t)(data->SystemStatus().MainInputVoltage().get()*1000.0);
      telemetry->system.fiveV = (uint16_t)(data->SystemStatus().ServoInputVoltage().get()*1000.0);
      telemetry->system.availableForCommand = data->SystemStatus().AvailableForCommand().get();

      fifo_msg.src.s_addr = shmPtr->clients.getAddr(a_client).s_addr; 
      fifo_msg.dest.s_addr = shmPtr->clients.getAddr(me).s_addr; 
      CMD_CMD(fifo_msg.data) = CMD_TELEM;
      CMD_NUM(fifo_msg.data) = sizeof(Telemetry_t);
      CMD_CSUM(fifo_msg.data) = 0;
      CMD_CSUM(fifo_msg.data) = C_calcCheckSum(fifo_msg.data);
      fifo_msg.size = CMD_SIZE(fifo_msg.data);

      n=writeFifo(INBOARD_TX);
    }
    else {
      GPSTelem_t * telemetry = (GPSTelem_t *)dataPtr;

      telemetry->pos.lat = data->GPS().Latitude().get();
      telemetry->pos.lon = data->GPS().Longitude().get();
      telemetry->track = data->GPS().Track().get();
      telemetry->pos.alt = data->GPS().Height().get();
      telemetry->sog = data->GPS().SOG().get();
      telemetry->utc.hours = data->GPS().UTCHours().get();
      telemetry->utc.minutes = data->GPS().UTCMinutes().get();
      telemetry->utc.seconds = data->GPS().UTCSeconds().get();

      fifo_msg.src.s_addr = shmPtr->clients.getAddr(a_client).s_addr; 
      fifo_msg.dest.s_addr = shmPtr->clients.getAddr(me).s_addr; 
      CMD_CMD(fifo_msg.data) = CMD_GPS_TELEM;
      CMD_NUM(fifo_msg.data) = sizeof(GPSTelem_t);
      CMD_CSUM(fifo_msg.data) = 0;
      CMD_CSUM(fifo_msg.data) = C_calcCheckSum(fifo_msg.data);
      fifo_msg.size = CMD_SIZE(fifo_msg.data);

      n=writeFifo(INBOARD_TX);
    }
  }
	catch(::xsd::cxx::tree::unexpected_element<char>  &e){}// not matched
	catch(...){cout << "Malformatted XML \n" << endl << flush; return;}

	try {
		std::auto_ptr<AtmosphericData::AtmosphericData_type> data (AtmosphericData::AtmosphericData(*document));

    Sonde_t * sonde;

    int payload_size=0;
    int num = 0;

    for (AtmosphericData::AtmosphericData_type::PTHSensor_const_iterator i (data->PTHSensor ().begin ());
        i != data->PTHSensor ().end ();
        ++i) {
      sonde = &((Sonde_t *)(dataPtr))[num];

      sonde->id = i->SensorID();
      sonde->gps.utc.hours = i->GPS().UTCHours().get();
      sonde->gps.utc.minutes = i->GPS().UTCMinutes().get();
      sonde->gps.utc.seconds = i->GPS().UTCSeconds().get();
      sonde->gps.pos.lat = i->GPS().Latitude().get();
      sonde->gps.pos.lon = i->GPS().Longitude().get();
      sonde->gps.track = i->GPS().Track().get();
      sonde->gps.pos.alt = i->GPS().Height().get();
      sonde->gps.sog = i->GPS().SOG().get();

      sonde->pth.pressure = i->AirData().Pressure().get();
      sonde->pth.temp = i->AirData().Temperature().get();
      sonde->pth.internal_temp = i->AirData().InternalTemperature().get();
      sonde->pth.rh1 = i->AirData().RelativeHumidity1().get();
      sonde->pth.rh2 = i->AirData().RelativeHumidity2().get();

      num++;
      payload_size += sizeof(Sonde_t);
		}

		String dest = data->VehicleID().c_str();
		Client * me = shmPtr->clients.getClient(0);
		Client * a_client = shmPtr->clients.getClient(dest);
    if(a_client) {
      fifo_msg.src.s_addr = shmPtr->clients.getAddr(a_client).s_addr; 
      fifo_msg.dest.s_addr = shmPtr->clients.getAddr(me).s_addr; 
      CMD_CMD(fifo_msg.data) = CMD_SONDE;
      CMD_NUM(fifo_msg.data) = payload_size;
      CMD_CSUM(fifo_msg.data) = 0;
      CMD_CSUM(fifo_msg.data) = C_calcCheckSum(fifo_msg.data);
      fifo_msg.size = CMD_SIZE(fifo_msg.data);

      n=writeFifo(INBOARD_TX);
    }
    else
      cout << "XMLtoData_Parser::update() - no such client: " << dest << endl;

  }
	catch(::xsd::cxx::tree::unexpected_element<char>  &e){}// not matched
	catch(...){cout << "Malformatted XML \n" << endl << flush; return;}

	try {
		std::auto_ptr<FlightPlan::FlightPlan_type> data (FlightPlan::FlightPlan(*document));

    Waypt_t * waypoint;

    int payload_size=0;
    int num = 0;

    for (FlightPlan::FlightPlan_type::Waypoint_const_iterator i (data->Waypoint ().begin ());
        i != data->Waypoint ().end ();
        ++i) {
      waypoint = &((Waypt_t *)(dataPtr))[num];

      waypoint->pos.lat = i->Latitude().get();
      waypoint->pos.lon = i->Longitude().get();
      waypoint->number = i->SequenceNumber().get();
			waypoint->next = i->Next().get();
      waypoint->orbitRad = i->OrbitRadius().get();
			cout << i->SequenceNumber().get() << " " << flush;

      num++;
      payload_size += sizeof(Waypt_t);
    }

		String dest = data->VehicleID().c_str();
		Client * me = shmPtr->clients.getClient(0);
		Client * a_client = shmPtr->clients.getClient(dest);
    if(a_client) {
      fifo_msg.src.s_addr = shmPtr->clients.getAddr(a_client).s_addr; 
      fifo_msg.dest.s_addr = shmPtr->clients.getAddr(me).s_addr; 
      CMD_CMD(fifo_msg.data) = CMD_FP_SUBS;
      CMD_NUM(fifo_msg.data) = payload_size;
      CMD_CSUM(fifo_msg.data) = 0;
      CMD_CSUM(fifo_msg.data) = C_calcCheckSum(fifo_msg.data);
      fifo_msg.size = CMD_SIZE(fifo_msg.data);

      n=writeFifo(INBOARD_TX);
    }
    else
      cout << "XMLtoData_Parser::update() - no such client: " << dest << endl;
  }
	catch(::xsd::cxx::tree::unexpected_element<char>  &e){}// not matched
	catch(...){cout << "Malformatted XML \n" << endl << flush; return;}

	try {
		std::auto_ptr<LinkStatus::LinkStatus_type> data (LinkStatus::LinkStatus(*document));
		cout << "Link Status Not Handled Yet" << endl;
  }
	catch(::xsd::cxx::tree::unexpected_element<char>  &e){}// not matched
	catch(...){cout << "Malformatted XML \n" << endl << flush; return;}

	try {
		std::auto_ptr<TaskStatus::TaskStatus_type> data (TaskStatus::TaskStatus(*document));
		cout << "Task Status Not Handled Yet" << endl;
  }
	catch(::xsd::cxx::tree::unexpected_element<char>  &e){}// not matched
	catch(...){cout << "Malformatted XML \n" << endl << flush; return;}

	try {
		std::auto_ptr<WindData::WindData_type> data (WindData::WindData(*document));

    WindData_t * wind = (WindData_t *)dataPtr;

    for (WindData::WindData_type::WindSensor_const_iterator i (data->WindSensor().begin ());
        i != data->WindSensor().end ();
        ++i) {
      wind->gps.utc.hours = i->GPS().UTCHours().get();
      wind->gps.utc.minutes = i->GPS().UTCMinutes().get();
      wind->gps.utc.seconds = i->GPS().UTCSeconds().get();
      wind->gps.pos.lat = i->GPS().Latitude().get();
      wind->gps.pos.lon = i->GPS().Longitude().get();
      wind->gps.track = i->GPS().Track().get();
      wind->gps.pos.alt = i->GPS().Height().get();
      wind->gps.sog = i->GPS().SOG().get();

      wind->south = i->WindComponents().South().get();
      wind->west = i->WindComponents().West().get();
      break; //FIXME only handling one sensor for now
    }

		String dest = data->VehicleID().c_str();
		Client * me = shmPtr->clients.getClient(0);
		Client * a_client = shmPtr->clients.getClient(dest);
    if(a_client) {
      fifo_msg.src.s_addr = shmPtr->clients.getAddr(a_client).s_addr; 
      fifo_msg.dest.s_addr = shmPtr->clients.getAddr(me).s_addr; 
      CMD_CMD(fifo_msg.data) = CMD_WIND;
      CMD_NUM(fifo_msg.data) = sizeof(WindData_t);
      CMD_CSUM(fifo_msg.data) = 0;
      CMD_CSUM(fifo_msg.data) = C_calcCheckSum(fifo_msg.data);
      fifo_msg.size = CMD_SIZE(fifo_msg.data);

      n=writeFifo(INBOARD_TX);
    }
    else
      cout << "XMLtoData_Parser::update() - no such client: " << dest << endl;
  }
	catch(::xsd::cxx::tree::unexpected_element<char>  &e){}// not matched
	catch(...){cout << "Malformatted XML \n" << endl << flush; return;}

	delete parser;
	delete errHandler;

}

XMLtoData_Parser::~XMLtoData_Parser()
{
	xercesc::XMLPlatformUtils::Terminate();
}
